﻿using System.Collections.Generic;
using System.Linq;
using System.Windows;
using System.Windows.Media;
using DynamicGeometry;

namespace PolylineRouting
{
    public  class RouteHelper
    {

        //List<Point> Points;

        public static List<Microsoft.Xna.Framework.Vector2> GetRoute(Microsoft.Xna.Framework.Vector2 start, Microsoft.Xna.Framework.Vector2 end, List<Microsoft.Xna.Framework.Vector2> vectors)
        {
            List<Point> points = new List<Point>();
            foreach (var vect in vectors)
            {
                points.Add(new Point(vect.X, vect.Y));
            }
            var polyline = RoutingAlgorithm.Dijkstra(new Point(start.X, start.Y), new Point(end.X, end.Y), new List<Point>(points));
            points = new List<Point>();
            PolylineRounding(polyline, points);

            vectors = new List<Microsoft.Xna.Framework.Vector2>();
            foreach (var point in points)
            {
                vectors.Add(new Microsoft.Xna.Framework.Vector2((float)point.X, (float)point.Y));
            }
            //Points = points;
            return vectors;
        }



        //public static IList<Point> GetRoute(Point start, Point end, List<Point> points)
        //{

        //    var polyline = RoutingAlgorithm.Dijkstra(start, end, new List<Point>(points));
        //    PolylineRounding(polyline, points);
        //    return polyline;
        //}

        //protected void ConstructPolyline(List<Point> points, List<Point> pathSegments)
        //{
        //    //PolylineRounding(points, pathSegments);
        //    //PolylineDirect(points);
        //}

        public static void PolylineRounding(List<Point> points, List<Point> segments)
        {
            double radius = 30;
            if (points.Count < 2)
            {
                return;
            }
            else if (points.Count == 2)
            {
                segments.Add(points[1]);
                return;
            }

            segments.Add(points[0]);

            int previousSign = Math.VectorProduct(points[0], points[1], points[2]).Sign();
            var tangentPoints = Math.GetTangentPoints(points[0], points[1], radius);
            Point previousPoint;
            if (previousSign > 0)
            {
                previousPoint = tangentPoints.P1;
            }
            else
            {
                previousPoint = tangentPoints.P2;
            }
            segments.Add(previousPoint);

            for (int i = 2; i < points.Count - 1; i++)
            {
                Point p1 = new Point();
                Point p2 = new Point();
                int sign = Math.VectorProduct(points[i - 1], points[i], points[i + 1]).Sign();
                if (sign == 0)
                {
                    p2 = points[i];
                }
                else if (sign == 1 && previousSign == 1)
                {
                    var vector = Math.RotatePoint(
                        points[i - 1],
                        radius,
                        (points[i - 1].AngleTo(points[i]) - Math.PI / 2)).Minus(points[i - 1]);
                    p1 = points[i - 1].Plus(vector);
                    p2 = points[i].Plus(vector);
                    //segments.Add(new ArcSegment()
                    //{
                    //    SweepDirection = SweepDirection.Clockwise,
                    //    Size = new Size(radius, radius),
                    //    Point = p1,
                    //    IsLargeArc = false
                    //});
                    segments.Add(p1);
                }
                else if (sign == -1 && previousSign == -1)
                {
                    var vector = Math.RotatePoint(
                        points[i - 1],
                        radius,
                        (points[i - 1].AngleTo(points[i]) + Math.PI / 2)).Minus(points[i - 1]);
                    p1 = points[i - 1].Plus(vector);
                    p2 = points[i].Plus(vector);
                    //segments.Add(new ArcSegment()
                    //{
                    //    SweepDirection = SweepDirection.Counterclockwise,
                    //    Size = new Size(radius, radius),
                    //    Point = p1,
                    //    IsLargeArc = false
                    //});
                    segments.Add(p1);
                }
                else if (previousSign == -1 && sign == 1)
                {
                    var midpoint = Math.Midpoint(points[i - 1], points[i]);
                    tangentPoints = Math.GetTangentPoints(midpoint, points[i - 1], radius);
                    p1 = tangentPoints.P1;
                    p2 = p1.Reflect(midpoint);
                    //segments.Add(new ArcSegment()
                    //{
                    //    SweepDirection = SweepDirection.Counterclockwise,
                    //    Size = new Size(radius, radius),
                    //    Point = p1,
                    //    IsLargeArc = false
                    //});
                    segments.Add(p1);
                }
                else if (previousSign == 1 && sign == -1)
                {
                    var midpoint = Math.Midpoint(points[i - 1], points[i]);
                    tangentPoints = Math.GetTangentPoints(midpoint, points[i - 1], radius);
                    p1 = tangentPoints.P2;
                    p2 = p1.Reflect(midpoint);
                    //segments.Add(new ArcSegment()
                    //{
                    //    SweepDirection = SweepDirection.Clockwise,
                    //    Size = new Size(radius, radius),
                    //    Point = p1,
                    //    IsLargeArc = false
                    //});
                    segments.Add(p1);
                }
                segments.Add(p2);
                previousPoint = p2;
                previousSign = sign;
            }

            tangentPoints = Math.GetTangentPoints(points[points.Count - 1],
                points[points.Count - 2],
                radius);
            if (previousSign == 1)
            {
                //segments.Add(new ArcSegment()
                //{
                //    SweepDirection = SweepDirection.Clockwise,
                //    Size = new Size(radius, radius),
                //    Point = tangentPoints.P2,
                //    IsLargeArc = false
                //});
                segments.Add(tangentPoints.P2);
                segments.Add(points[points.Count - 1]);
            }
            else
            {
                //segments.Add(new ArcSegment()
                //{
                //    SweepDirection = SweepDirection.Counterclockwise,
                //    Size = new Size(radius, radius),
                //    Point = tangentPoints.P1,
                //    IsLargeArc = false
                //});
                segments.Add(tangentPoints.P1);
                segments.Add(points[points.Count - 1]);
            }
        }

    }
}